#pragma once
#include <iostream>
#include <Eigen/Dense>
#include <memory>
#include "continuous_linear_system.h"
#include "discrete_linear_system.h"
using namespace LinearMpc;
namespace auto_ros
{
namespace control
{
class VehicleLinearLv : public ContinuousLinearSys
{
public:
	//init state fun when speed is 1
	VehicleLinearLv(double mass, double cf, double cr, double lf, double lr, double ix, double iz, double h, double k_phi, double d_phi, double max_lateral_acc = 9, double max_tire_f_angle = 0.5);
	void speed_update_matrix(double speed);
	double mass();
	double lf();
	double lr();
	double cf();
	double cr();
	double iz();
	double max_tire_f_angle();
	double speed_threshold();
	Eigen::MatrixXd base_state_matrix();
	bool set_mass(double mass);
	bool set_lf(double lf);
	bool set_lr(double lr);
	bool set_cf(double cf);
	bool set_cr(double cr);
	bool set_iz(double iz);
	bool set_max_lateral_acc(double max_acc);
	bool set_max_tire_f_angle(double max_angle);

private:
	//paramter
	double mass_;
	double cf_;
	double cr_;
	double lf_;
	double lr_;
	double ix_;
	double iz_;
	double h_;
	double k_phi_;
	double d_phi_;
	//local parameter
	double theta1_;
	double theta2_;
	double theta3_;
	double theta4_;
	double theta5_;
	//steady state and control variable
	Eigen::VectorXd steady_state_;
	Eigen::VectorXd steady_control_;
	double max_lateral_acc_;
	double max_tire_f_angle_;
	//state
	double speed_;
	double speed_threshold_; // divided by 0 error
	Eigen::MatrixXd base_state_matrix_;
};
} // namespace control
} // namespace auto_ros